#ifndef __OPERATION_TREE_HPP__
#define __OPERATION_TREE_HPP__
/////////////////////////////////////////////////////
// 通过树搜索获得一套求解操作
#include <algorithm>
#include <cmath>
#include "../base.hpp"
// #include "randomDropOut.hpp"

typedef struct OperationNode
{
    int prevIndex; // 上一步操作的 ID
    int step;      //  这一步的最小深度

    vector<vector<bool>> mask; // 可联通区域
    vector<Point2D> box;       // 箱子的位置
    vector<bool> bucket;       // 人物的位置
    long code;                 // 对箱子的位置进行编码，方便查找
} OperationNode;

/// @brief Dijkstra 算法获取箱子的移动方案
class OperationTree
{
public:
    // 下面是三个超参数
    int extraExploreDetph;  // 额外搜索节点深度
    bool showLog;           // 是否显示逐步探索的记录
    bool wantBestSolution;  // 是否加入额外探索的节点来做优化
    bool ifSearchInAllNode; // 是否在所有可能的节点中探索

    vector<vector<bool>> map;
    vector<vector<int>> distMap;

    vector<vector<bool>> reachableAreas;
    vector<Point2D> reachablePoints;
    int posNum;
    int maxDepth;

    vector<Point2D> initBox;
    vector<Point2D> target;
    Point2D startPoint;

    vector<OperationNode> allNode;
    vector<int> unexpandedIndex;

    // 优化，空间换时间
    vector<int> expandedIndex;
    int compareBeginIndex;

    vector<int> answerIndex;
    vector<vector<Point2D>> ansBoxList;

    OperationTree()
    {
        extraExploreDetph = 3;
        wantBestSolution = true;
        showLog = true;
        ifSearchInAllNode = false;
    };

    /// @brief 初始化
    void init(_State &state)
    {
        unexpandedIndex.clear();
        allNode.clear();
        ansBoxList.clear();
        answerIndex.clear();

        initBox = state.box;
        startPoint = state.man;
        map = state.map;
        target = state.target;

        initMask(reachableAreas);
        flood(map, reachableAreas, state.man);
        reachablePoints.clear();
        for (int i = 0; i < reachableAreas[0].size(); i++)
            for (int j = 0; j < reachableAreas.size(); j++)
                if (reachableAreas[j][i])
                    reachablePoints.push_back({i, j});
        posNum = reachablePoints.size();

        maxDepth = (posNum - target.size()) * target.size() * 3 / 2;
        // maxDepth = 60;

        distMap.clear();
        for (int i = 0; i < map.size(); i++)
        {
            vector<int> l;
            for (int j = 0; j < map[0].size(); j++)
                l.push_back(-1);
            distMap.push_back(l);
        }
        for (int i = 0; i < target.size(); i++)
            flood2(map, distMap, target[i], 0);
    }

    /// @brief 问题求解
    bool findAnswer(_State &state)
    {
        int index = -1;
        bool flag = false;

        int endIndex;
        _State endState = state;
        endState.box = endState.target;
        // 初始化
        cout << "initializing ... " << endl;
        init(state);
        if (ifSearchInAllNode)
            findAllNode();
        else
            findNodeWithDFS(state);
        cout << "totally node =" << allNode.size() << endl;
        // saveBox("./log.txt");

        long endEncode = EncodeBoxPose(endState.box);
        //// 把第一个元素加入列表
        index = findNodeIndexInAllNode(state, true);
        cout << index << " - first node :";
        printBoxes(state.box);
        if (index == -1)
        {
            cerr << "Fail to find first node!!!!!!!" << endl;
            return false;
        }
        expandedIndex.push_back(index);

        for (int i = 0; i < unexpandedIndex.size(); i++)
            if (unexpandedIndex[i] == index)
            {
                unexpandedIndex.erase(unexpandedIndex.begin() + i);
                break;
            }
        /// 主循环
        // cout << "-------------------" << endl;
        /// 启动主循环

        compareBeginIndex = 0;
        vector<int> neighbors;
        while (expandedIndex.size() < allNode.size())
        {
            // 相邻节点的下标
            index = -1;
            neighbors.clear();
            for (int i = compareBeginIndex; i < unexpandedIndex.size(); i++)
            {
                // 寻找新节点的入相邻节点
                for (int j = 0; j < expandedIndex.size(); j++)
                {
                    if (isConnected(allNode[expandedIndex[j]], allNode[unexpandedIndex[i]]))
                        neighbors.push_back(j);
                }

                if (neighbors.size() > 0)
                {
                    index = unexpandedIndex[i];
                    unexpandedIndex.erase(unexpandedIndex.begin() + i);
                    compareBeginIndex = i;
                    break;
                }
            }
            // cout<<"_______"<<compareBeginIndex<<","<<index<<endl;
            if (index == -1 && compareBeginIndex == 0)
                break;
            if (index == -1)
            {
                compareBeginIndex = 0;
                continue;
            }
            compareBeginIndex /= 2;
            if (showLog)
                cout << allNode.size() << " \\ " << expandedIndex.size()
                     << " \t i = " << index
                     << " \t Near =" << neighbors.size()
                     << " \t code = " << allNode[index].code << endl;
            /// 跟新深度
            // 更新新节点深度
            allNode[index].step = allNode[expandedIndex[neighbors[0]]].step + 1;
            allNode[index].prevIndex = expandedIndex[neighbors[0]];
            for (int i = 1; i < neighbors.size(); i++)
                if (allNode[index].step > allNode[expandedIndex[neighbors[i]]].step + 1)
                {
                    allNode[index].step = allNode[expandedIndex[neighbors[i]]].step + 1;
                    allNode[index].prevIndex = expandedIndex[neighbors[i]];
                }
            // cout << "checkPoint 2" << endl;
            // 寻找新节点的出相邻节点
            neighbors.clear();
            for (int j = 0; j < expandedIndex.size(); j++)
            {
                // if (generateRandomNumber(0, 100) < 20)
                //     continue;   // 随机选择是否优化，提高速度
                if (isConnected(allNode[index], allNode[expandedIndex[j]]))
                    neighbors.push_back(j);
            }
            // 更新相邻节点深度
            for (int i = 1; i < neighbors.size(); i++)
                if (allNode[index].step + 1 < allNode[expandedIndex[neighbors[i]]].step)
                {
                    allNode[expandedIndex[neighbors[i]]].step = allNode[index].step + 1;
                    allNode[expandedIndex[neighbors[i]]].prevIndex = index;
                }
            /// 添加
            expandedIndex.push_back(index);
            // // 提前终止
            if (!wantBestSolution && endEncode == allNode[index].code)
                break; // 找到解就退出
        }

        /// 可添加的节点都添加完毕，寻找结束节点
        endIndex = findNodeIndexInAllNode(endState, false);
        cout << "end node index = " << endIndex << endl;
        cout << " end node prevIndex = " << allNode[endIndex].prevIndex << endl;
        if (endIndex == -1 || allNode[endIndex].prevIndex == -1)
        {
            cout << "End node = ";
            printBoxes(endState.box);
            cerr << "Fail to find end node!!!!!!!" << endl;
            return false;
        }

        /// 把结果装填进 answerIndex
        answerIndex.clear();
        answerIndex.push_back(endIndex);
        while (allNode[endIndex].prevIndex != -1)
        {
            // cout<<allNode[endIndex].prevIndex<<endl;
            answerIndex.push_back(allNode[endIndex].prevIndex);
            endIndex = allNode[endIndex].prevIndex;
        }
        reverse(answerIndex.begin(), answerIndex.end());

        // cout << "checkPoint 4" << endl;
        return true;
    }

    //////////////////////////////////
    // 找到所有的节点
    /// @brief 寻找所有可能的状态并存储在 allNode 中
    void findAllNode()
    {
        allNode.clear();
        vector<bool> emptybucket;
        vector<Point2D> box;
        vector<int> boxPositions; // 箱子的位置初始化
        for (int i = 0; i < target.size(); i++)
        {
            boxPositions.push_back(i);
            box.push_back({0, 0});
        }
        for (int i = 0; i < posNum; i++)
            emptybucket.push_back(false);

        while (1)
        {
            // 初始化新节点
            OperationNode opn;
            opn.prevIndex = -1;
            opn.step = -1;

            // 更新箱子的位置
            opn.bucket = emptybucket;
            for (int i = 0; i < boxPositions.size(); i++)
            {
                box[i] = reachablePoints[boxPositions[i]];
                opn.bucket[boxPositions[i]] = true;
            }

            opn.box = box;
            opn.code = EncodeBoxPose(box);
            opn.step = countMatch(box);

            if (!isStacked(opn))
            { // 找空位
                int posIndex = -1;
                for (int i = 0; i < posNum; i++)
                {
                    bool flag = true;
                    for (int j = 0; j < boxPositions.size(); j++)
                        if (boxPositions[j] == i)
                        {
                            flag = false;
                            break;
                        }
                    if (flag)
                    {
                        posIndex = i;
                        break;
                    }
                }
                // 更新可达区域
                vector<vector<bool>> tmpMap = map;
                for (int i = 0; i < boxPositions.size(); i++)
                    tmpMap[reachablePoints[boxPositions[i]].y][reachablePoints[boxPositions[i]].x] = true;
                initMask(opn.mask);
                flood(tmpMap, opn.mask, reachablePoints[posIndex]);
                // 添加
                allNode.push_back(opn);
                // 检查图是否为全通,如果不是就需要进一步探索
                int countReachable = 0;
                for (int i = 0; i < opn.mask[0].size(); i++)
                    for (int j = 0; j < opn.mask.size(); j++)
                        if (opn.mask[j][i])
                            countReachable++;
                if (countReachable + box.size() < posNum)
                    ExpandNodeAtUnreachableAreas(opn, tmpMap);
            }
            // 如果到头了就退出
            if (!getNextBoxPosition(boxPositions))
                break;
        }
        unexpandedIndex.clear();
        for (int i = 0; i < allNode.size(); i++)
            unexpandedIndex.push_back(i);
        // // 索引排序
        // int tmp, ind;
        // cout << "start to rank nodes" << endl;
        // for (int i = 0; i < allNode.size(); i++)
        // {
        //     for (int j = i; j < allNode.size(); j += 7)
        //         if (allNode[unexpandedIndex[i]].step < allNode[unexpandedIndex[j]].step)
        //             ind = j;
        //     tmp = unexpandedIndex[i];
        //     unexpandedIndex[i] = unexpandedIndex[ind];
        //     unexpandedIndex[ind] = tmp;
        // }
    }

    /// @brief 扩展在非全通区域的节点
    void ExpandNodeAtUnreachableAreas(OperationNode &state, vector<vector<bool>> &tmpMap)
    {
        vector<OperationNode> nodes;
        // 探索
        for (int k = 0; k < posNum; k++) // 在空白区域中寻找
        {
            int i = reachablePoints[k].x;
            int j = reachablePoints[k].y;
            if (!state.mask[j][i])
            {
                // 确保新节点不和任何一个已经探索的节点相通
                bool flag = true;
                for (int t = 0; t < nodes.size(); t++)
                    if (nodes[t].mask[j][i])
                        flag = false;
                if (!flag)
                    continue;
                // 创建新节点
                OperationNode opn;
                Point2D from = {i, j};
                initMask(opn.mask);
                flood(tmpMap, opn.mask, from);
                nodes.push_back(opn);
            }
        }
        // 把找到的节点放进去
        for (int i = 0; i < nodes.size(); i++)
        {
            nodes[i].box = state.box;
            nodes[i].bucket = state.bucket;
            nodes[i].prevIndex = -1;
            nodes[i].step = state.step;
            nodes[i].code = state.code;
            allNode.push_back(nodes[i]);
        }
    }

    /// @brief 寻找下一个节点的箱子位置
    bool getNextBoxPosition(vector<int> &boxPosition)
    {
        // 如果只有一位就特殊处理
        if (boxPosition.size() == 1)
            if (boxPosition[0] < posNum - 1)
            {
                boxPosition[0]++;
                return true;
            }
            else
                return false;

        // 如果有多位,首尾和中间分别处理
        for (int i = boxPosition.size() - 1; i >= 0; i--)
        {
            if (i == boxPosition.size() - 1)
            { // 如果时末位
                if (boxPosition[i] < posNum - 1)
                { // 如果没到头就继续加
                    boxPosition[i]++;
                    return true;
                }
            }
            else
            { // 到头了就进位
                if (boxPosition[i] < boxPosition[i + 1] - 1)
                {
                    // 如果没到头就继续加
                    boxPosition[i]++;
                    for (int j = i + 1; j < boxPosition.size(); j++)
                        boxPosition[j] = boxPosition[j - 1] + 1;
                    return true;
                }
            }
        }
        // 进位进位到最后没得进了就退出
        return false;
    }

    ////////////////////////////////////////////////////
    // 带有启发的 DFS 算法查找节点
    void findNodeWithDFS(_State s)
    {
        // 加入起点
        allNode.clear();
        OperationNode opn = createOpnNode(s.box, s.man);
        maxDepth = (posNum - target.size()) * target.size() / 4;
        maxDepth = max(maxDepth, 6);
        allNode.push_back(opn);
        // 开始搜索
        bool flag = false;
        vector<int> indexList;
        flag = DFS(0);
        while (flag == false)
        {
            indexList.clear();
            for (int i = 0; i < allNode.size(); i++)
                if (allNode[i].step == maxDepth)
                    indexList.push_back(i);
            if(indexList.size() == 0)
                return;
            maxDepth = maxDepth * 5 / 4;
            for (int i = 0; i < indexList.size(); i++)
            {
                flag = DFS(indexList[i]);
                if (flag)
                    break;
            }
        }
        // 填充未探索列表
        unexpandedIndex.clear();
        expandedIndex.clear();
        for (int i = 0; i < allNode.size(); i++)
            unexpandedIndex.push_back(i);
    }

    /// @brief 深度优先搜索解决方案
    bool DFS(int prevIndex)
    {
        if (showLog)
        {
            cout << allNode.size() << "|"
                 << allNode[prevIndex].step << "/"
                 << maxDepth << " : ";
            printBoxes(allNode[prevIndex].box);
        }
        if (allNode[prevIndex].step >= maxDepth)
            return false;
        if (isEndNode(allNode[prevIndex]))
            return true;

        bool flag;
        int n;
        vector<vector<bool>> tmpmap;

        // 找到所有的下一步并进行评估
        vector<OperationNode> opns;
        vector<int> values;
        vector<int> indexes;
        int directions[4][2] = {{0, 1}, {0, -1}, {-1, 0}, {1, 0}};
        for (int i = 0; i < allNode[prevIndex].box.size(); i++)
        {
            for (int k = 0; k < 4; k++)
                if (isMoveAble(allNode[prevIndex], i, directions[k][0], directions[k][1]))
                {
                    OperationNode opn;
                    vector<Point2D> box = allNode[prevIndex].box;
                    box[i].x += directions[k][0];
                    box[i].y += directions[k][1];
                    opn = createOpnNode(box, allNode[prevIndex].box[i]);
                    opn.step = allNode[prevIndex].step + 1;
                    values.push_back(evaluateNode(opn.box));
                    if (isStacked(opn))
                        continue;
                    // 去重
                    if (findNodeIndexInAllNode(opn) == -1)
                    {
                        opns.push_back(opn);
                        indexes.push_back(allNode.size());
                        allNode.push_back(opn);
                    }
                }
        }

        // 根据评估结果进行升序排序
        vector<int> rankedIndex;
        for (int i = 0; i < opns.size(); i++)
        {
            n = 0;
            // 寻找最小值
            for (int j = 0; j < opns.size(); j++)
                if (values[n] > values[j])
                    n = j;
            rankedIndex.push_back(n);
            values[n] = 114514;
        }

        // 根据排序结果扩展节点
        bool haveFind = false;
        for (int i = 0; i < opns.size(); i++)
        {
            if (DFS(indexes[rankedIndex[i]]))
            {
                if (extraExploreDetph == 0)
                    return true;
                haveFind = true;
                maxDepth = extraExploreDetph;
                for (int i = 0; i < opns.size(); i++)
                    opns[i].step = 0;
            }
        }
        if (haveFind)
            return true;
        return false;
    }

    /// @brief 判断一步动作是否可进行
    bool isMoveAble(OperationNode &s, int &boxIndex, int &dx, int &dy)
    {
        Point2D b = s.box[boxIndex];
        if (!s.mask[b.y - dy][b.x - dx])
            return false;
        if (map[b.y + dy][b.x + dx])
            return false;
        b = {b.x + dx, b.y + dy};
        for (Point2D b2 : s.box)
            if (b == b2)
                return false;
        return true;
    }

    /// @brief 启发函数，评估箱子和目标点的距离
    int evaluateNode(vector<Point2D> box)
    {
        int val = 0;
        for (Point2D b : box)
        {
            val += distMap[b.y][b.x];
        }
        return val;
    }

    OperationNode createOpnNode(vector<Point2D> boxes, Point2D man)
    {
        int flag;
        vector<vector<bool>> tmpmap;

        OperationNode opn;
        rankBox(boxes);

        opn.box = boxes;
        opn.code = EncodeBoxPose(opn.box);

        opn.step = 0;
        opn.prevIndex = -1;

        for (int i = 0; i < posNum; i++)
        {
            flag = false;
            for (int s = 0; s < boxes.size(); s++)
                if (reachablePoints[i] == opn.box[s])
                    flag = true;
            opn.bucket.push_back(flag);
        }

        initMask(opn.mask);
        tmpmap = map;
        for (int i = 0; i < boxes.size(); i++)
            tmpmap[boxes[i].y][boxes[i].x] = true;
        flood(tmpmap, opn.mask, man);
        return opn;
    }
    ////////////////////////////////////////////////////

    /// @brief 判断两个节点之间是否相通，即从 state1 能否移动到 state2
    bool isConnected(OperationNode &state1, OperationNode &state2)
    {
        // 找出位置不同的一组箱子
        int index1 = -1, index2 = -1;
        bool flag1, flag2;
        for (int i = 0; i < posNum; i++)
        {
            flag1 = state1.bucket[i];
            flag2 = state2.bucket[i];
            if (flag1 && !flag2)
            {
                if (index1 != -1)
                    return false;
                index1 = i;
            }
            if (!flag1 && flag2)
            {
                if (index2 != -1)
                    return false;
                index2 = i;
            }
            // if (index1 != -1 && index2 != -1)
            //     break;
        }
        if (index1 == -1 || index2 == -1)
            return false;
        // cout<<"index :"<<index1<<" "<<index2<<endl;
        // 检查对应移动是否可实现
        Point2D box1 = reachablePoints[index1];
        Point2D box2 = reachablePoints[index2];
        int dx = box2.x - box1.x;
        int dy = box2.y - box1.y;
        if ((abs(dx) + abs(dy)) != 1)
            return false;
        // cout<<"Its near. dx,dy="<<dx<<" , "<<dy<<endl;
        // 检查对应位置是否可达
        int tox = box1.x - dx;
        int toy = box1.y - dy;
        // cout<<tox<<','<<toy<<" Is reachable: "<<state1.mask[toy][tox]<<endl;
        if ((!state1.mask[toy][tox]) || (!state2.mask[box1.y][box1.x]))
            return false;
        return true;
    }

    /// @brief 判断是不是最终的节点
    bool isEndNode(OperationNode &state)
    {
        for (Point2D &box : state.box)
        {
            bool flag = false;
            for (Point2D &targ : target)
                if (box == targ)
                    flag = true;
            if (!flag)
                return false;
        }
        return true;
    }

    /// @brief 检查这个状态是不是有方块卡住了，旨在减少节点的个数
    bool isStacked(OperationNode &state)
    {
        int countBorder[4] = {0, 0, 0, 0};
        int flag1, flag2;
        bool isInTarget[state.box.size()];
        for (int i = 0; i < state.box.size(); i++)
        {
            Point2D box = state.box[i];
            // 检查上下和左右是不是至少各有一堵墙
            flag1 = map[box.y - 1][box.x] || map[box.y + 1][box.x];
            flag2 = map[box.y][box.x - 1] || map[box.y][box.x + 1];
            if (flag1 && flag2)
            { // 检查是不是已经就位了
                flag1 = false;
                for (Point2D &t : target)
                    if (t == box)
                    {
                        flag1 = true;
                        break;
                    }
                isInTarget[i] = flag1;
                if (!flag1)
                    return true;
            }
            if (box.x == 1)
                countBorder[0]++;
            else if (box.x == map[0].size() - 2)
                countBorder[1]++;
            if (box.y == 1)
                countBorder[2]++;
            else if (box.y == map.size() - 2)
                countBorder[3]++;
        }
        // cout << countBorder[0] << "," << countBorder[1] << ","
        //      << countBorder[2] << "," << countBorder[3] << endl;
        // return false;
        // 检查是否被推到边线
        for (Point2D &t : target)
        {
            if (t.x == 1)
                countBorder[0]--;
            else if (t.x == map[0].size() - 2)
                countBorder[1]--;
            if (t.y == 1)
                countBorder[2]--;
            else if (t.y == map.size() - 2)
                countBorder[3]--;
        }
        if (countBorder[0] > 0 ||
            countBorder[1] > 0 ||
            countBorder[2] > 0 ||
            countBorder[3] > 0)
            return true;
        return false;
    }

    /// @brief 漫水填充法计算可达区域
    void flood(vector<vector<bool>> &map_, vector<vector<bool>> &mask, Point2D &from)
    {
        if (map_[from.y][from.x])
            return;
        if (mask[from.y][from.x])
            return;
        mask[from.y][from.x] = true;

        Point2D next = {from.x + 1, from.y};
        if (mask[0].size() > next.x && !mask[next.y][next.x])
            flood(map_, mask, next);

        next = {from.x - 1, from.y};
        if (0 < next.x && !mask[next.y][next.x])
            flood(map_, mask, next);

        next = {from.x, from.y + 1};
        if (mask.size() > next.y && !mask[next.y][next.x])
            flood(map_, mask, next);

        next = {from.x, from.y - 1};
        if (0 < next.y && !mask[next.y][next.x])
            flood(map_, mask, next);
        return;
    }

    /// @brief 漫水填充法填充最短距离
    void flood2(vector<vector<bool>> &map_, vector<vector<int>> &mask, Point2D &from, int step)
    {
        if (map_[from.y][from.x])
            return;
        if (mask[from.y][from.x] == -1 || mask[from.y][from.x] < step)
            return;

        mask[from.y][from.x] = step;

        Point2D next = {from.x + 1, from.y};
        if (mask[0].size() > next.x && !mask[next.y][next.x])
            flood2(map_, mask, next, step + 1);

        next = {from.x - 1, from.y};
        if (0 < next.x && !mask[next.y][next.x])
            flood2(map_, mask, next, step + 1);

        next = {from.x, from.y + 1};
        if (mask.size() > next.y && !mask[next.y][next.x])
            flood2(map_, mask, next, step + 1);

        next = {from.x, from.y - 1};
        if (0 < next.y && !mask[next.y][next.x])
            flood2(map_, mask, next, step + 1);
        return;
    }

    //////////////////////////////////////
    /// @brief 把得到的结果翻译成路径,奇数到偶数位置要推动箱子
    vector<Point2D> translateToPath()
    {
        vector<Point2D> path;
        path.push_back(startPoint);
        // for (int i = 0; i < answerIndex.size(); i++)
        // {
        //     cout << answerIndex[i] << "\t";
        //     printBoxes(allNode[answerIndex[i]].box);
        // }
        for (int i = 1; i < answerIndex.size(); i++)
        {
            OperationNode state1 = allNode[answerIndex[i - 1]];
            OperationNode state2 = allNode[answerIndex[i]];
            // 找出位置不同的一组箱子
            int index1 = -1, index2 = -1;
            bool flag1, flag2;
            for (int i = 0; i < posNum; i++)
            {
                flag1 = state1.bucket[i];
                flag2 = state2.bucket[i];
                if (flag1 && !flag2)
                    index1 = i;

                if (!flag1 && flag2)
                    index2 = i;

                if (index1 != -1 && index2 != -1)
                    break;
            }
            Point2D box1 = reachablePoints[index1];
            Point2D box2 = reachablePoints[index2];
            int dx = box2.x - box1.x;
            int dy = box2.y - box1.y;
            int tox = box1.x - dx;
            int toy = box1.y - dy;
            // 添加位置点
            path.push_back({tox, toy});
            path.push_back({box1.x, box1.y});
        }
        ansBoxList.clear();
        for (int i = 0; i < answerIndex.size(); i++)
        {
            ansBoxList.push_back(allNode[answerIndex[i]].box);
            ansBoxList.push_back(allNode[answerIndex[i]].box);
        }
        return path;
    }

    void initMask(vector<vector<bool>> &mask)
    {
        mask = map;
        for (int i = 0; i < mask.size(); i++)
            for (int j = 0; j < mask[0].size(); j++)
                mask[i][j] = false;
    }

    long EncodeBoxPose(vector<Point2D> &box)
    {
        long code = 0;
        long w = map[0].size();
        long h = map.size();
        long weight = 1;
        for (int i = 0; i < box.size(); i++)
        {
            code += box[i].x * weight;
            weight *= w;
            code += box[i].y * weight;
            weight *= h;
        }
        return code;
    }

    /// @brief 对箱子进行排序
    void rankBox(vector<Point2D> &box)
    {
        vector<int> Bucket;
        for (int i = 0; i < posNum; i++)
            Bucket.push_back(0);
        for (int i = 0; i < box.size(); i++)
            for (int j = 0; j < posNum; j++)
                if (box[i] == reachablePoints[j])
                    Bucket[j]++;
        box.clear();
        for (int i = 0; i < posNum; i++)
            if (Bucket[i] > 0)
                box.push_back(reachablePoints[i]);
    }

    /// @brief 在总节点列表中搜索目标状态
    int findNodeIndexInAllNode(_State &state, bool ifCheckMan)
    {
        int n;
        // 对箱子的位置进行排序
        rankBox(state.box);
        long code = EncodeBoxPose(state.box);
        // 寻找第一个对应的节点
        for (int i = 0; i < allNode.size(); i++)
        {
            if (allNode[i].code == code)
            {
                if (ifCheckMan && !(allNode[i].mask[state.man.y][state.man.x]))
                    continue;
                // 考虑到箱子数量太多时会出现编码溢出的情况，所以重新确认以下箱子的对应性
                n = state.box.size();
                for (int k = 0; k < state.box.size(); k++)
                    for (int j = 0; j < allNode[i].box.size(); j++)
                        if (state.box[k] == allNode[i].box[j])
                        {
                            n--;
                            break;
                        }
                if (n == 0)
                    return i;
            }
        }
        return -1;
    }

    int findNodeIndexInAllNode(OperationNode &s)
    {
        int n;

        for (int i = 0; i < allNode.size(); i++)
            if (allNode[i].code == s.code)
            {
                // 初筛后确认连通性
                for (int x = 0; x < map[0].size(); x++)
                    for (int y = 0; y < map.size(); y++)
                        if (s.mask[y][x] != allNode[i].mask[y][x])
                            return false;
                return i;
            }
        return -1;
    }
    ////////////////////////////////////////////

    void printBoxes(vector<Point2D> &box)
    {
        cout << EncodeBoxPose(box) << " : ";
        for (int i = 0; i < box.size(); i++)
            cout << box[i].x << " " << box[i].y << " | ";
        cout << endl;
    }

    void printMask(vector<vector<bool>> &mask)
    {
        for (int i = 0; i < mask.size(); i++)
        {
            for (int j = 0; j < mask[0].size(); j++)
                cout << mask[i][j] << ' ';
            cout << endl;
        }
    }

    void saveBox(string filename)
    {
        ofstream fout(filename);
        for (int i = 0; i < allNode.size(); i++)
        {
            vector<Point2D> box = allNode[i].box;
            fout << EncodeBoxPose(box) << " : ";
            for (int i = 0; i < box.size(); i++)
                fout << box[i].x << " " << box[i].y << " | ";
            fout << endl;
        }
        fout.close();
    }

    int countMatch(vector<Point2D> box)
    {
        int count = 0;
        for (Point2D b : box)
            for (Point2D t : target)
                if (b == t)
                    count++;
        return count;
    }
};

////////////////////////////////////////////////////
#endif